﻿#include "Matrix.h"
#include "Constant.h"
#include "CoordSys.h"
#include "TimeSys.h"
#include "ReadN.h"
#include "ReadO.h"
#include "GNSSSatPos.h"
#include "IonoCorr.h"
#include "TropCorr.h"
#include "Spp.h"
#include <iostream>
#include <iomanip>
#include <vector>
#include <cstdlib> /*用于使用语句 system("pause");*/
using namespace std;

/* 用于测试类 */
int main() {
    /* 测试常数类 */
    GNSSconsts* pGnssConst;
    BDSConsts bdsConst;
    pGnssConst = &bdsConst; /* 用法：基类指针指向派生类实例化对象 */
    cout << fixed << pGnssConst->get_refEllip().f << endl;

    /* 测试矩阵类 */
    cout << "/* 测试矩阵类 */" << endl;
    Matrix matTest;
    matTest.test();

    /* 测试坐标系统类 */
    cout << "/* 测试坐标系统类 */" << endl;
    CoordSys* pCoordSys;
    BDCS bdcs(bdsConst.get_refEllip());
    pCoordSys = &bdcs;
    pCoordSys->test();

    /* 测试时间系统类 */
    cout << "/* 测试时间系统类 */" << endl;
    CalendT t = { 2016,7,14,1,0,0 };/* N 文件中第一个星历的时间，对应BDS 549周， 349200秒 */
    BDSTime mBDST;
    TimeSys* pTimeSys = &mBDST;
    pTimeSys->print_time(t);
    cout << " 对应的BDS时间为：" << endl;
    SOW t2 = pTimeSys->calend2sow(t);
    pTimeSys->print_time(t2);

    /* 测试 Read N 文件类 */
    ReadN mReadN("C:\\1234560660442.17N");
    if (!mReadN.parseFile()) return false;
    if (!mReadN.print_eph()) return false;

    /* 测试 Read O 文件类 */
    string filename = "c:/1234560660442.17O";// 声明要读取的文件名   
    ReadO mReadO(filename);  // 创建 ReadO 对象
    if (!mReadO.parse_file()) return false;
    mReadO.print_head();// 显示文件头部信息
    mReadO.print_body(2);// 显示前 10 个历元的观测数据
    vector<obsEpoch>* mObs = mReadO.get_obs();
    BaseT tr;
    tr = pTimeSys->str2baseT(mObs->at(0).time); // 获取O文件中第一次观测的的时间

    /* 测试卫星位置计算类 */
    cout << "/* 卫星位置计算类 */" << endl;
    GNSSSatPos* pSatPos;
    BDSSatPos mBDSSatPos(pTimeSys, &mReadN);
    pSatPos = &mBDSSatPos; /* 用法：基类指针指向派生类实例化对象 */
    XYZ ps1;
    ps1 = pSatPos->get_satPos("C01", tr);
    printf("C01 卫星位置：X=%.3f m, Y=%.3f m, Z=%.3f m\n\n", ps1.x, ps1.y, ps1.z);

    /* 电离层延迟误差改正 */
    cout << "/*电离层延迟误差改正类 */" << endl;
    IonoCorr mIonCor(pGnssConst, &mReadN.get_head());
    CalendT tObs = { 2017, 3, 7, 5, 6, 35,0.00 };
    SOW sow = pTimeSys->calend2sow(tObs);

    XYZ rr = { -2091342.6543,4800418.4462,3629758.3665 }; /* 接收机坐标概况值 */
    BLH blh = pCoordSys->XYZ2BLH(rr);         /* 接收机大地坐标 */
    Azel azel = pCoordSys->get_azel(rr, ps1); /* 计算接收机到卫星的方位角和高度角 */
    double ionCor = mIonCor.get_ionoCorr(blh, azel, sow);
    cout << fixed << ionCor << endl << endl;

    /* 对流层延迟误差改正 */
    cout << "/*对流层延迟误差改正类 */" << endl;
    TropCorr mTropCor(pGnssConst);
    double tropCor = mTropCor.get_tropCorr(blh, azel.ele);
    cout << fixed << tropCor << endl << endl;
}


/* 单点定位数据处理全过程：
1. 读取RINEX O文件和N文件
2. 初始化接收机近似坐标
3. 计算卫星位置
4. 计算电离层和对流层延迟改正
5. 构建观测方程
6. 最小二乘解算接收机坐标
7. 迭代计算直到收敛
8. 输出最终定位结果
*/

// 标准单点定位算法实现
int spp(const string& nfile, const string& ofile) {
    // 1. 读取RINEX O文件和N文件
    ReadN mReadN(nfile);
    if (!mReadN.parseFile()) {
        cerr << "N文件解析失败" << endl;
        return EXIT_FAILURE;
    }
    
    ReadO mReadO(ofile);
    if (!mReadO.parse_file()) {
        cerr << "O文件解析失败" << endl;
        return EXIT_FAILURE;
    }
    
    // 2. 初始化接收机近似坐标
    XYZ rr = { -2091342.6543,4800418.4462,3629758.3665 };
    
    // 3. 计算卫星位置
    GNSSSatPos* pSatPos;
    BDSSatPos mBDSSatPos(pTimeSys, &mReadN);
    pSatPos = &mBDSSatPos;
    
    // 获取观测数据
    vector<obsEpoch>* mObs = mReadO.get_obs();
    
    // 验证UTC时间转换
    CalendT utcTime = {2017, 3, 7, 5, 6, 35, 0.0};
    BaseT baseTime = pTimeSys->calend2baseT(utcTime);
    cout << "UTC时间验证: " << utcTime.year << "-" << utcTime.month << "-" << utcTime.day 
         << " " << utcTime.hour << ":" << utcTime.minute << ":" << utcTime.second << endl;
    
    // 4. 计算误差改正
    IonoCorr mIonCor(pGnssConst, &mReadN.get_head());
    TropCorr mTropCor(pGnssConst);
    
    // 验证卫星位置计算
    XYZ ps = pSatPos->get_satPos(mObs->at(0).prn, baseTime);
    cout << "卫星位置验证: X=" << ps.x << ", Y=" << ps.y << ", Z=" << ps.z << endl;
    cout << "参考值: X=-32348377, Y=27042006, Z=509548" << endl;
    
    // 5. 构建观测方程
    Matrix A(mObs->size()*2, 4); // 设计矩阵
    Matrix L(mObs->size()*2, 1); // 观测向量
    Matrix P(mObs->size()*2, mObs->size()*2); // 权矩阵
    
    // 初始化权矩阵为单位矩阵
    for(int i = 0; i < P.get_row(); i++) {
        P(i,i) = 1.0;
    }
    
    // 6. 最小二乘解算
    for(int iter = 0; iter < 10; iter++) {
        // 重置矩阵
        A.zero();
        L.zero();
        
        // 构建观测方程
        for(size_t i = 0; i < mObs->size(); i++) {
            // 计算卫星位置
            XYZ ps = pSatPos->get_satPos(mObs->at(i).prn, mObs->at(i).time);
            
            // 计算几何距离
            double rho = sqrt(pow(ps.x-rr.x,2) + pow(ps.y-rr.y,2) + pow(ps.z-rr.z,2));
            
            // 计算方位角和高度角
            BLH blh = pCoordSys->XYZ2BLH(rr);
            Azel azel = pCoordSys->get_azel(rr, ps);
            
            // 计算误差改正
            double ionCor = mIonCor.get_ionoCorr(blh, azel, mObs->at(i).time);
            double tropCor = mTropCor.get_tropCorr(blh, azel.ele);
            
            // 构建设计矩阵和观测向量
            double dx = (ps.x - rr.x)/rho;
            double dy = (ps.y - rr.y)/rho;
            double dz = (ps.z - rr.z)/rho;
            
            // 伪距观测方程
            A(i*2, 0) = dx;
            A(i*2, 1) = dy;
            A(i*2, 2) = dz;
            A(i*2, 3) = 1.0;
            L(i*2, 0) = mObs->at(i).P1 - rho - ionCor - tropCor;
            
            // 载波相位观测方程
            A(i*2+1, 0) = dx;
            A(i*2+1, 1) = dy;
            A(i*2+1, 2) = dz;
            A(i*2+1, 3) = 1.0;
            L(i*2+1, 0) = mObs->at(i).L1 - rho - ionCor - tropCor;
        }
        
        try {
            // 解算方程
            Matrix dx = (A.transpose()*P*A).inverse() * A.transpose()*P*L;
            
            // 更新接收机坐标
            rr.x += dx(0,0);
            rr.y += dx(1,0);
            rr.z += dx(2,0);
            
            // 检查收敛条件
            if(dx.norm() < 0.001) {
                cout << "迭代收敛于第" << iter+1 << "次迭代" << endl;
                break;
            }
        } catch(...) {
            cerr << "矩阵求逆失败，请检查观测数据" << endl;
            return EXIT_FAILURE;
        }
    }
    
    // 8. 输出结果
    printf("最终定位结果：X=%.3f m, Y=%.3f m, Z=%.3f m\n", rr.x, rr.y, rr.z);
    return EXIT_SUCCESS;
}